from inside import *
from draw import *
from forward import *
import numpy as np

#杆件编号, 杆件扭角 (alpha), 杆件长度 (a), 关节距离 (d), 关节角 (theta)
#




DH_params = np.array([
    [1, -45, 0, 560, 12],
[2, 45, 0, 450, 0],
[3, 45, 0, 410, 10],
[4, -45, 0, 380, -20],
[5, 45, 0, 360, 0],
[6, 0, 0, 210, 40]
])
all_DH_params = [DH_params]

# 迭代5次
for _ in range(100):
    # 复制当前矩阵
    new_DH_params = all_DH_params[-1].copy()

    # 第二列（扭角 alpha）每个元素减少5
    new_DH_params[:, 1] -= 5
    # 第五列（关节角 theta）每个元素减少3

    # 检查并调整超出范围的角度
    new_DH_params[:, 1] = np.clip(new_DH_params[:, 1], -90, 90)
    new_DH_params[:, 4] = np.clip(new_DH_params[:, 4], -90, 90)

    # 如果有值被限制在边界上，则进行调整
    if np.any(new_DH_params[:, 1] == -90) or np.any(new_DH_params[:, 1] == 90):
        new_DH_params[:, 1] += 5
    if np.any(new_DH_params[:, 4] == -90) or np.any(new_DH_params[:, 4] == 90):
        new_DH_params[:, 4] += 6

    # 将新矩阵添加到列表中
    all_DH_params.append(new_DH_params)

count = 0
for DH_params in all_DH_params:
    # plot_robot_arm(DH_params)
    T = forward_kinematics(DH_params)
    print('DH参数求得齐次坐标变换矩阵 T:')
    print(T)

    # 初始角度
    q0 = [0, 0, 0, 0, 0, 0]

    q = inverse_kinematics(T, q0, DH_params)
    # print('逆运动学求得关节角向量为：')
    # print(q)

    DH_params[:, 4] = q  # 这里我们更新最后一列，即关节角theta

    T_test = forward_kinematics(DH_params)
    print('逆运动学求得齐次坐标变换矩阵 T_test:')
    print(T_test)
    dif = np.abs(T - T_test)
    print('误差:', dif)
    if np.allclose(T, T_test, atol=8.0):
        count +=1
print('精度', count/100)
count = 0

